#include "SubsData.h"

SubsData::SubsData(string topic_suffix) :
		sensors_data( { { 0 } }) {

	// topic published to each subscriber, informing the relevant sensor
	sensors_topics[0] = "/Sensors_Module/Front_Laser/";
	sensors_topics[1] = "/Sensors_Module/FrontRight_Laser/";
	sensors_topics[2] = "/Sensors_Module/FrontLeft_Laser/";
	sensors_topics[3] = "/Sensors_Module/Right_Laser/";
	sensors_topics[4] = "/Sensors_Module/Right_US/";
	sensors_topics[5] = "/Sensors_Module/Left_Laser/";
	sensors_topics[6] = "/Sensors_Module/Left_US/";
	sensors_topics[7] = "/Sensors_Module/Back_Laser/";
	sensors_topics[8] = "/Sensors_Module/Back_US/";
	sensors_topics[9] = "/Sensors_Module/Front_Laser_Range/";

	this->topic_suffix = topic_suffix;
}

void SubsData::subscribe(const short& sensor_index, const float& min_dist, const float& max_dist) {

	if (sensors_names::FRONT_LASER_RANGE != sensor_index) {

		// set publisher for indicated sensor
		publishers[sensor_index] = m_node_handler.advertise<std_msgs::String>(sensors_topics[sensor_index] + topic_suffix, 1000);
	} else {

		// set publisher for indicated sensor
		publishers[sensor_index] = m_node_handler.advertise<std_msgs::Float32MultiArray>(sensors_topics[sensor_index] + topic_suffix, 1000);

	}

	// set subscription on
	sensors_data[IS_SUBS_INDEX][sensor_index] = IS_SUBS;

	// set lower notification bound
	sensors_data[MIN_CRITICAL_DIST_INDEX][sensor_index] = min_dist;

	// set high notification bound
	sensors_data[MAX_CRITICAL_DIST_INDEX][sensor_index] = max_dist;
}

void SubsData::deSubscribe(const short& sensor_index) {

	// set subscription on
	sensors_data[IS_SUBS_INDEX][sensor_index] = NOT_SUBS;
}

bool SubsData::isSubscribed(const short& sensor_index) const {
	return IS_SUBS == sensors_data[IS_SUBS_INDEX][sensor_index];
}

bool SubsData::isSubscribed() {

	for(int i = 0; i < sensors_names::NUM_ITEMS; ++i) {

		if (isSubscribed(i)) {
			return true;
		}
	}

	return false;
}

void SubsData::getRange(const short& sensor_index, float& min, float& max) const {

	min = sensors_data[MIN_CRITICAL_DIST_INDEX][sensor_index];
	max = sensors_data[MAX_CRITICAL_DIST_INDEX][sensor_index];
}
